//
// Created by kevin on 2020/5/7.
//
#include "bino/bino_toolbox.h"
#include "bino/config.h"

using namespace std;
using namespace Eigen;
clock_t start,en;
// 文件路径
std::string config_file = "/home/kevin/UAV/load_ws/src/bi2pcl/config/default.yaml";


int main(int argc, char **argv) {

    ros::init(argc, argv, "bi2pc");
    kbino::Bi2PC b2p(config_file);
    kbino::BiRosInterface node;

    while(!ros::isShuttingDown()){
        // Start clock
        start = clock();

        cv::Mat left = node.getLeft();
        cv::Mat right = node.getRight();
//	cv::Mat left = cv::imread("/home/kevin/workSpace/cs294_ws/src/bi2pcl/data/left.png",0);
//	cv::Mat right = cv::imread("/home/kevin/workSpace/cs294_ws/src/bi2pcl/data/right.png",0);

	if(left.empty()||right.empty()){
	        cout<<"No Image!"<<endl;
        ros::spinOnce();
        continue;
	}


    else{
        b2p.bi2pc(left,right);
        cv::Mat dis = b2p.getDis()/ 96.0;
        node.PcPublish(b2p.getPointCloud());
        node.imagePublish(dis);
        cv::imshow("Depth",dis);
        cv::imshow("Original",left);
        cv::imshow("Originalr",right);
        cv::waitKey(1);
//        b2p.showInPango();
        en= clock();   //结束时间
//        cout<<"Freq = "<<CLOCKS_PER_SEC/(double)(en-start)<<"Hz"<<endl;
    }
    ros::spinOnce();

    }
    return 0;
}



